#pragma config(Hubs,  S1, HTServo,  HTMotor,  HTMotor,  HTMotor)
#pragma config(Motor,  motorA,          A,             tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  motorB,          B,             tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  motorC,          C,             tmotorNormal, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     A1,            tmotorNormal, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     A2,            tmotorNormal, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C3_1,     A3,            tmotorNormal, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C3_2,     A4,            tmotorNormal, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C4_1,     A5,            tmotorNormal, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C4_2,     A6,            tmotorNormal, openLoop, encoder)
#pragma config(Servo,  srvo_S1_C1_1,    S1,                   tServoNormal)
#pragma config(Servo,  srvo_S1_C1_2,    S2,                   tServoNormal)
#pragma config(Servo,  srvo_S1_C1_3,    S3,                   tServoNormal)
#pragma config(Servo,  srvo_S1_C1_4,    S4,                   tServoNormal)
#pragma config(Servo,  srvo_S1_C1_5,    S5,                   tServoNormal)
#pragma config(Servo,  srvo_S1_C1_6,    S6,                   tServoNormal)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//----(C) Copyright 2012. All rights reserved. Allyn Xu. -----

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.

task main()
{

  //Motor A1 and A3 are Reversed

  //Motor Drive
  int speedcontrol = 1;
  int drive1speed;
  int drive2speed;
  bool reverse = false;

  //Ball Flipping
  bool isMovingL = false;
  bool isMovingR = false;
  int ballFlipUpL = 2;
  int ballFlipUpR = 2;
  nMotorEncoder[A] = 540;
  nMotorEncoder[B] = 540;
  bool isReseting = false;

	//Crate Lift
  bool independentCtrl = false;
	int liftspeedcontrol = 2;
  int liftspeed;
  int maxLift = 3960;
  int minLift = -1000;
  nMotorEncoder[A3] = 0;
  nMotorEncoder[A4] = 0;

  //Crate Tension
  bool topHatReset = false;
  motor[C] = 0;

  //Crate Flip
	bool frontFlip = true;

	int verticalUpFront = 190;
	int verticalDownFront = 105;
	int horizontalOutFront = 220;
	int horizontalInFront = 160;

	int verticalUpBack = 70;
	int verticalDownBack = 165;
	int horizontalOutBack = 30;
	int horizontalInBack = 70;

  servoChangeRate[S1] = 2;
  servoChangeRate[S2] = 2;
  servoChangeRate[S3] = 2;
  servoChangeRate[S4] = 2;
  wait1Msec(500);
  servo[S2] = horizontalOutFront;
  servo[S4] = horizontalOutBack;
  wait1Msec(500);
	servo[S1] = verticalUpFront;
  servo[S3] = verticalUpBack;



  while(true)
  {

    getJoystickSettings(joystick);

//=============================== Joy 1 ================================

    //---------------- Speed Control ----------------
    if(joystick.joy1_TopHat != -1)
    {
      speedcontrol = joystick.joy1_TopHat + 1;
      wait1Msec(100);
    }
    //---------------- End Speed Control ----------------


    //---------------- Motor Drive ----------------
    int debug_motorA1 = nMotorEncoder[A1];
    int debug_motorA2 = nMotorEncoder[A2];

    if(joy1Btn(10) == 1)
    {
      nMotorEncoder[A1] = 0;
      nMotorEncoder[A2] = 0;
    }

    drive1speed = joystick.joy1_y1/speedcontrol;
    drive2speed = joystick.joy1_y2/speedcontrol;

    if(joy1Btn(1) == 1 && reverse == false)
    {
      reverse = true;
    }

    if(joy1Btn(2) == 1 && reverse == true)
    {
      reverse = false;
    }

    if(reverse == true)
    {
      int drive1temp = drive1speed;
      drive1speed = -drive2speed;
      drive2speed = -drive1temp;
    }

    motor[A1] = 1.25 * drive1speed;
    motor[A2] = drive2speed;
    //---------------- End Motor Drive ----------------

    //---------------- Ball Flipping ----------------

    int debugmotor = nMotorEncoder[A];

    //Motor Reset
    if(joy1Btn(9) == 1)
    {
      if(!isReseting)
      {
	      isReseting = true;
			  isMovingL = false;
			  isMovingR = false;
			  ballFlipUpL = 0;
			  ballFlipUpR = 0;
			  motor[A] = -25;
			  motor[B] = -25;
			}
    }

    if(joy1Btn(9) && isReseting)
    {
      motor[A] = 0;
      motor[B] = 0;
      isReseting = false;
		  nMotorEncoder[A] = 0;
		  nMotorEncoder[B] = 0;
    }

    if(isMovingL)
    {
      if(nMotorEncoder[A] >= 540 && ballFlipUpL == 2)
      {
        motor[A] = 0;
        isMovingL = false;
      } else if(nMotorEncoder[A] >= 275 && ballFlipUpL == 1) {
        motor[A] = 0;
        isMovingL = false;
      }

      if(nMotorEncoder[A] <= 0 && ballFlipUpL == 0)
      {
        motor[A] = 0;
        isMovingL = false;
      }
    }

    if(!isMovingL && !isReseting)
    {
      if(joy1Btn(5) == 1 && ballFlipUpL != 2) //Go up
      {
        motor[A] = 50;
        isMovingL = true;
        if(nMotorEncoder[A] < 100)
        {
          ballFlipUpL = 1;
        } else {
          ballFlipUpL = 2;
        }
      } else if(joy1Btn(7) == 1 && ballFlipUpL != 0) { //Go down
        motor[A] = -50;
        ballFlipUpL = 0;
        isMovingL = true;
      }
    }


    if(isMovingR)
    {
      if(nMotorEncoder[B] >= 540 && ballFlipUpR == 2)
      {
        motor[B] = 0;
        isMovingR = false;
      }else if(nMotorEncoder[B] >= 275 && ballFlipUpR == 1){
        motor[B] = 0;
        isMovingR = false;
      }

      if(nMotorEncoder[B] <= 0 && ballFlipUpR == 0)
      {
        motor[B] = 0;
        isMovingR = false;
      }
    }

    if(!isMovingR && !isReseting)
    {
      if(joy1Btn(6) == 1 && ballFlipUpR != 2) //Go up
      {
        motor[B] = 50;
        //ballFlipUpR = true;
        isMovingR = true;
        if(nMotorEncoder[B] < 100)
        {
          ballFlipUpR = 1;
        } else {
          ballFlipUpR = 2;
        }
      } else if(joy1Btn(8) == 1 && ballFlipUpR != 0) { //Go down
        motor[B] = -50;
        ballFlipUpR = 0;
        isMovingR = true;
      }
    }


    //---------------- End Ball Flipping --------------



//=============================== Joy 2 ================================

    //-------------- Crate Lifting ----------------
    int debug_motorA3 = nMotorEncoder[A3];
    int debug_motorA4 = nMotorEncoder[A4];


    if(joy2Btn(1) == 1)
    {
      independentCtrl = false;
    }
    if(joy2Btn(2) == 1)
    {
      independentCtrl = true;
    }

    if(!independentCtrl)
    {
      liftspeed = joystick.joy2_y1/liftspeedcontrol;
      if( !(nMotorEncoder[A3] > -15000 && nMotorEncoder[A3] < -10000 && liftspeed >= 0) )
      {
        motor[A3] = liftspeed;
      }
      if( !(nMotorEncoder[A4] > -15000 && nMotorEncoder[A4] < -10000 && liftspeed >= 0) )
      {
        motor[A4] = liftspeed;
      }
    } else {
      liftspeed = joystick.joy2_y1/liftspeedcontrol;
      if( !(nMotorEncoder[A3] > -15000 && nMotorEncoder[A3] < -10000 && liftspeed >= 0) )
      {
        motor[A3] = liftspeed;
      }
      liftspeed = joystick.joy2_y2/liftspeedcontrol;
      if( !(nMotorEncoder[A4] > -15000 && nMotorEncoder[A4] < -10000 && liftspeed >= 0) )
      {
        motor[A4] = liftspeed;
      }

    }


    /*
    if(nMotorEncoder[A3] < maxLift && nMotorEncoder[A3] > minLift)
    {
      motor[A3] = liftspeed;
      motor[A4] = liftspeed;
    } else if(nMotorEncoder[A3] > maxLift && liftspeed < 0) {
      motor[A3] = liftspeed;
      motor[A4] = liftspeed;
    } else if(nMotorEncoder[A3] < minLift && liftspeed > 0) {
      motor[A3] = liftspeed;
      motor[A4] = liftspeed;
    } else {
      motor[A3] = 0;
      motor[A4] = 0;
    }
    */

    //---------------- End Crate Lifting ----------------


    //---------------- Lift Tension ----------------
    int debugBtn = joystick.joy2_TopHat;

    if(joy2Btn(6) == 0 && joy2Btn(8) == 0)
    {
      motor[C] = 0;
    }
    if(joy2Btn(6) == 1)
    {
      motor[C] = 50;
    }
    if(joy2Btn(8) == 1)
    {
      motor[C] = -50;
    }


    //---------------- End Lift Tension ----------------



    //---------------- Crate Flipping ----------------

    //Swap
    if(joy2Btn(5) == 1)
    {
      frontFlip = true;
    }
    if(joy2Btn(7) == 1)
    {
      frontFlip = false;
    }

    //Vertical
    if(joystick.joy2_TopHat == 4) //Go down
    {
      if(frontFlip)
      {
        servo[S1] = verticalDownFront;
      } else {
        servo[S3] = verticalDownBack;
      }
      wait1Msec(500);
    }
    if(joystick.joy2_TopHat == 0) //Go Up
    {
      if(frontFlip)
      {
        servo[S1] = verticalUpFront;
      } else {
        servo[S3] = verticalUpBack;
      }
      wait1Msec(500);
    }

    //Horizontal
    if(joystick.joy2_TopHat == 2) //Go in
    {
      if(frontFlip)
      {
        servo[S2] = horizontalInFront;
      } else{
        servo[S4] = horizontalInBack;
      }
      wait1Msec(500);
    }
    if(joystick.joy2_TopHat == 6) //Go out
    {
      if(frontFlip)
      {
        servo[S2] = horizontalOutFront;
      } else {
        servo[S4] = horizontalOutBack;
      }
      wait1Msec(500);
    }
    //---------------- End Crate Flipping ----------------

/*
    //---------------- Crate Grabbing ----------------
    if(joy2Btn(1) == 1)
    {
      if(crategrab == true)
      {
        servo[S4] = 90; //open
        crategrab = false;
        wait1Msec(300);
      } else {
        servo[S4] = 230; //closed
        crategrab = true;
        wait1Msec(300);
      }
    }
    //---------------- End Crate Grabbing ----------------
*/


  }
}
